#include "Estimator.hpp"
#include <array>
namespace visionhw
{

    Estimator::Estimator()
        : _cloud_src(new CloudType),
          _cloud_tgt(new CloudType),
          _fpfh_src(new pcl::PointCloud<pcl::FPFHSignature33>),
          _fpfh_tgt(new pcl::PointCloud<pcl::FPFHSignature33>),
          _sac_ia(),
          _sac_result(new CloudType),
          _sac_trans(),
          _icp_result(new CloudType),
          _icp()
    {
        init();
    }

    void Estimator::setInputSource(const CloudType::Ptr cloud_source,
                                   const pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfh_source)
    {
        *_cloud_src = *cloud_source;
        *_fpfh_src = *fpfh_source;
    }

    void Estimator::setInputTarget(const CloudType::Ptr cloud_target,
                                   const pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfh_target)
    {
        *_cloud_tgt = *cloud_target;
        *_fpfh_tgt = *fpfh_target;
    }

    void Estimator::init()
    {
    }

    void Estimator::estimate(CloudType &cloud_output)
    {
        // sac-ia
        _sac_ia.setInputSource(_cloud_src);
        _sac_ia.setInputTarget(_cloud_tgt);
        _sac_ia.setSourceFeatures(_fpfh_src);
        _sac_ia.setTargetFeatures(_fpfh_tgt);
        auto t1 = std::chrono::high_resolution_clock::now();
        _sac_ia.align(*_sac_result);
        auto t2 = std::chrono::high_resolution_clock::now();
        _sac_time_used_ms = std::chrono::duration<double, std::milli>(t2 - t1).count();

        std::cout << "sac has converged:" << _sac_ia.hasConverged() << "  score: " << _sac_ia.getFitnessScore() << "\n";
        _sac_trans = _sac_ia.getFinalTransformation();
        std::cout << _sac_trans << "\n";
        std::cout << "sac_ia time used:" << _sac_time_used_ms << " ms\n";
        std::cout << "-- -------sac_ia end-------- --" << std::endl;

        if (_saveIntermediatePly)
            pcl::io::savePLYFile("outputply/粗配准后.ply", *_sac_result);

        // icp
        _icp.setInputSource(_cloud_src);
        _icp.setInputTarget(_cloud_tgt);
        t1 = std::chrono::high_resolution_clock::now();
        _icp.align(*_icp_result, _sac_trans);
        t2 = std::chrono::high_resolution_clock::now();
        _icp_time_used_ms = std::chrono::duration<double, std::milli>(t2 - t1).count();
        std::cout << "_icp has converged:" << _icp.hasConverged() << "  score: " << _icp.getFitnessScore() << "\n";
        _icp_trans = _icp.getFinalTransformation();
        std::cout << _icp_trans << "\n";
        std::cout << "-------INVERSE TRANSFORM MATRIX(4*4)--------------------\n";
        std::cout << _icp_trans.inverse() << "\n";

        std::string filename = Config::getValue("fileDir")["filename"].asString();
        std::cout << "Current Filename: " << filename <<"\n";

        // 配准矩阵转化为欧拉角
        std::cout << "-------INVERSE TRANSFORM EULERANGLE & TRANSLATION-------\nEulerAngle:\n";
        std::cout << _icp_trans.inverse().topLeftCorner<3,3>().eulerAngles(2, 1, 0) << "\nTranslation:\n";
        std::cout << _icp_trans.inverse().topRightCorner<3,1>() << "\n";

        // label转化为欧拉角
        std::cout << "-------LABEL EULERANGLE & TRANSLATION-------------------\nEulerAngle:\n";
        Eigen::Matrix3d labelR;
        Eigen::Array3d labelt;
        std::array<double, 9> R{0};
        std::array<double, 3> t{0};
        auto jsonR = Config::getValue("label")[filename]["R"];
        auto jsont = Config::getValue("label")[filename]["t"];
        for (int i = 0; i < jsonR.size(); ++i){
            R[i] = jsonR[i].asDouble();
        }
        for (int i = 0; i < jsont.size(); ++i){
            t[i] = jsont[i].asDouble();
        }
        labelR << R[0], R[1], R[2], R[3], R[4], R[5], R[6], R[7], R[8];
        labelt << t[0], t[1], t[2];

        std::cout << labelR.eulerAngles(2, 1, 0) << "\nTranslation:\n";
        std::cout << labelt << "\n";
        
        std::cout << "icp time used:" << _icp_time_used_ms << " ms\n";
        std::cout << "-- ---------icp end--------- --" << std::endl;

        if (_saveIntermediatePly)
            pcl::io::savePLYFile("outputply/精细配准后.ply", *_icp_result);

        cloud_output = *_icp_result;
    }

}